#include "stm32f4xx.h"
#include "rtthread.h"
#include "drv_can.h"
#include "fir_filter.h"
#include "app_sensor.h"
#include "drv_st485.h"
#include "HexapodServo.h"
#include <string.h>
#include <stdlib.h>
//#include "hexapod_leg.h"
#include "hexapod.h"

#define SPEED_FILTER_TAP      2 //FIR filter depth
#define RCT_FILTER_TAP 				2 //FIR filter depth
#define MSPEEDLIMT 						300 // 300RPM				
#define TAEGET_PITCH_ANGLE_LIMT 3.0 //Target pitch angle limit
#define PITCH_ANGLE_PROTECT_LIMIT 30 //pitch angle over 30锟斤拷 will disable motors for protection
#define ROLL_ANGLE_PROTECT_LIMIT  10 //roll angle over 10锟斤拷 will disable motors for protection
#define TARGET_TORQUE_LIMIT 25000  //25000mA
#define SBUS_ROBOT_ENABLE_CH	8//sbus Enable switch channel in RC_CONTROL mode
#define SBUS_CONTSOUCE_CH	 4	//speed contrlo source from sbus
#define SBUS_HEADER_CH		 3  //sbus header angle channel
#define SBUS_SPEED_CH      1	//sbus speed cmd channel
#define SBUS_YAW_CH        0	//sbus yaw speed cmd channel
#define SBUS_VALUE_BASE 1000	//sbus signal base
#define BALANCE_ANGLE      0	//robot balance angle
#define BALANCE_ANGLE_TOLERANCE	0.2//robot balance angle tolerance
#define TX2_CONSOUCE_VALUE 306
#define RC_CONTORL_CALUE   1000
#define USB_UARTSOUCE_VALUE 1694
#define ROBOT_STOP_SBUS_VALUE 1694
#define ROBOT_START_SBUS_VALUE 306

typedef enum
{
	MOTOR_IDLE_MODE = 0,
	MOTOR_TORQUE_MODE,
	MOTOR_OPEN_LOOP_MODE,
	MOTOR_SPEED_MODE,
	MOTOR_CALI_MECH_ZERO_MODE,//4
	MOTOR_FREE_STOP_MODE,			
	MOTOR_BREAK_STOP_MODE,
	BLDC_MOTOR_PWM,
}MC_CTRL_MODE_TYPE;

typedef enum
{
	RC_CONTROL = 0,
	USB_UART,
	TX2_UART,
	CAN,
}CONTORL_SOURCE_TYPE;

typedef enum
{
	ROBOT_INIT = 0,
	SELT_BALANCE,
	POSE_CONTROL,
	SPEED_CONTROL,
	ROBOT_STOP,
	ERR_STOP,
}ROBOT_STATUS_TYPE;

typedef struct
{
	float pitch_angle;
	float pitch_angle_rate;
	float yaw_angle_rate;
	float torque;
	float diff_torque;
	float accMSpeed;
	float Mspeed;
	u16Params MspeedRPM;
	u16Params MTorque_mA;
	float header_angle;
}CONTORL_TARGET;

extern union {
		float speed;
		u8 data[4];
}Setlinespeed;

extern union {
		float speed;
		u8 data[4];
}Setrotatespeed;
extern union {
		float angle;
		u8 data[4];
}Headerangle;;

MC_CTRL_MODE_TYPE 	MotortStatus = MOTOR_IDLE_MODE;		//inital motor status is idle
CONTORL_SOURCE_TYPE ControlSource = RC_CONTROL;				//inital controlsource is RC
ROBOT_STATUS_TYPE		Robotstatus		=  ROBOT_INIT;						//inital robotstatus is Init
CONTORL_TARGET Target_value ={0};
float TotalMspeed;
union {
	floatParams Feedback_Mspeed;
	u8 					data[8];
}WheelReadSpeedPack;
//floatParams Feedback_Mspeed;
//u8 ST_SendBuff[20];
extern SYSTEM_STATUS g_tSysStatus ;    

static floatParams Pitch_AccMspeedPID = {0.3,0.05};//pose (pitch)-motor accelspeed PI params
static floatParams Pitch_TorquePID = {6000,800};//{6000,800};//pose(pitch)-torque PI params
static floatParams YawRate_TorqueDiffPID = {250,0.1};//pose (yaw)-torque PI params
static floatParams Speed_PitchTargetPID ={0.12,0.0005};//speed-pose(target pitch angle)PI params
const float c_fFactorMspeed[SPEED_FILTER_TAP] ={0.3,0.7};//speed FIR filter ,depth = 2 
const float c_fFactorRCTspeed[RCT_FILTER_TAP] = {0.5,0.5};//remote controllor speed command filter

static CAN_msg CanSendPackage;

static float s_fMspeedFilter_Delay[SPEED_FILTER_TAP];//?????????
static float s_fRCTargetSpeedFilter_Delay[RCT_FILTER_TAP];//????????????????
static FIR_FILTER_Float_DEF  s_tMspeedFilter = 
		{s_fMspeedFilter_Delay, c_fFactorMspeed, SPEED_FILTER_TAP};//??FIR?????
static FIR_FILTER_Float_DEF  s_tRCTargetSpeedFliter = 
		{s_fRCTargetSpeedFilter_Delay, c_fFactorRCTspeed,RCT_FILTER_TAP};//????????????

		
extern rt_sem_t rt_sem_motion ;
extern u16 SBUS_ch_value[16];

static u8 DRIVER_CAN_SyncModePack[8] = {
	0x2b,0x0f,0x20,0x00,0x01,0x00,0x00,0x00};
static u8 DRIVER_CAN_SpeedModePack[8] = {
	0x2f,0x60,0x60,0x00,0x03,0x00,0x00,0x00};
static u8 DRIVER_CAN_TorqueModePack[8] = {
	0x2f,0x60,0x60,0x00,0x04,0x00,0x00,0x00};
//Left&Right motor accel and decel speed set as 100RPM/S
static u8 DRIVER_CAN_LM_SpeedAcctimePack[8] = {
	0x23,0x83,0x60,0x01,0x64,0x00,0x00,0x00};
static u8 DRIVER_CAN_RM_SpeedAcctimePack[8] = {
	0x23,0x83,0x60,0x02,0x64,0x00,0x00,0x00};
static u8 DRIVER_CAN_LM_SpeedDectimePack[8] = {
	0x23,0x84,0x60,0x01,0x64,0x00,0x00,0x00};
static u8 DRIVER_CAN_RM_SpeedDectimePack[8] = {
	0x23,0x84,0x60,0x02,0x64,0x00,0x00,0x00};
//left&right motor torque slope set as 1000mA/s
static u8 DRIVER_CAN_LM_TorqueaimtimePack[8]={
	0x23,0x87,0x60,0x01,0xE8,0X03,0X00,0X00};
static u8 DRIVER_CAN_RM_TorqueaimtimePack[8]={
	0x23,0x87,0x60,0x02,0xE8,0X03,0X00,0X00};
//motor start
static u8 DRIVER_CAN_Enable06Pack[8] = {
	0x2b,0x40,0x60,0x00,0x06,0x00,0x00,0x00};
static u8 DRIVER_CAN_Enable07Pack[8] = {
	0x2b,0x40,0x60,0x00,0x07,0x00,0x00,0x00};
static u8 DRIVER_CAN_Enable0fPack[8] = {
	0x2b,0x40,0x60,0x00,0x0f,0x00,0x00,0x00};
static u8 DRIVER_CAN_DisablePack[8] = {
	0x2b,0x40,0x60,0x00,0x00,0x00,0x00,0x00};
//motor speed send pack
static u8 DRIVER_CAN_SpeedSendPack[8]={
	0x23,0xff,0x60,0x03,0x64,0x00,0x64,0x00};
//motor torque send pack
static u8 DRIVER_CAN_TorqueSendPack[8]={
	0x23,0x71,0x60,0x03,0x00,0x00,0xFF,0xFF};
//cmd(read speed) speed pack  
static u8 DRIVER_CAN_SpeedfeedbackSendPack[8]={
	0x40,0x6C,0x60,0x03,0x00,0x00,0x00,0x00};
	
/*******************************************************************************
* Function Name : IntegralYawRateDiff
* Description   : yaw angle rate integral calculate
* Input         :   float target 		now targer value
										float nowvalue  now real value
										u8 clean_bit    clear inner integral value bit
* Output        : NONE
* Return        : float intergral_value ???
*******************************************************************************/
float IntegralYawRateDiff(float target,float nowvalue,u8 clean_bit)
{
	static float intergral_value = 0;
	
	if( 1 == clean_bit )
	{ 
		intergral_value = 0;
		return intergral_value;
	}
	else
	{
		intergral_value += (target - nowvalue);
		
		if( intergral_value > 40000)
			intergral_value = 40000;
		else if( intergral_value < -40000 )
			intergral_value = -40000;
		
		return 	intergral_value ;
	}
}
/*******************************************************************************
* Function Name : IntegralSpeedDiff
* Description   : speed integral calculate 
* Input         :   float target 		
										float nowvalue  
										u8 clean_bit    
* Output        : NONE
* Return        : float intergral_value 
*******************************************************************************/
float IntegralSpeedDiff(float target,float nowvalue,u8 clean_bit)
{
	static float intergral_value = 0;
	
	if( 1 == clean_bit )
	{ 
		intergral_value = 0;
		return intergral_value;
	}
	else
	{
		intergral_value += (target - nowvalue);
		
		if( intergral_value > 40000)
			intergral_value = 40000;
		else if( intergral_value < -40000 )
			intergral_value = -40000;
		
		return 	intergral_value ;
	}
}
		
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void U16value_Init(u16 *arry,u16 initvalue, u8 count)
{
	u8 i =0;
	for(i=0;i<count;i++)
	{
		* (arry + i) = initvalue;
	}
}
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void ClearTargetspeed(void)
{
		Target_value.Mspeed = 0;
		Target_value.yaw_angle_rate = 0;
		Target_value.header_angle = 0;
		Setlinespeed.speed = 0;
		Setrotatespeed.speed =0;
		Headerangle.angle =0;	
}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SET_ROBOT_STOP(void)
{
	if(Robotstatus != ERR_STOP)
	{
		Robotstatus = ROBOT_STOP;
		rt_kprintf("ROBOT Stop!\r\n");
	}
	else
	{
		rt_kprintf("ROBOT ERROR_STOP!\r\n");		
	}
	
}
/**
  * @name			ROBOT_START
  * @brief
  * @param  	none
  * @retval 	none
  */
void SET_ROBOT_START(void)
{
	if(ROBOT_INIT == Robotstatus)
	{
//		DRIVER_CAN_Init();//
		Robotstatus = SELT_BALANCE;
		rt_kprintf("ROBOT Start to balance mode\r\n");
	}
	if(ROBOT_STOP == Robotstatus)
	{
		Robotstatus = ROBOT_INIT;
		rt_kprintf("Robot status initilize to INIT!\r\n");
	}
	if(ERR_STOP == Robotstatus)
	{
		Robotstatus = ERR_STOP;
		rt_kprintf("Robot ERR! can not start!\r\n");	
	}
	if(	( SELT_BALANCE  == Robotstatus)||
			(	POSE_CONTROL == Robotstatus)||	
			( SPEED_CONTROL == Robotstatus)
		)
	{
		rt_kprintf("ROBOT Already Start!\r\n");
	}

}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void JudgeRobotStatuewhenRCmodeFromSbus(void)
{
	static u16 enprevalue = ROBOT_START_SBUS_VALUE;
	static u16 ennowvalue = ROBOT_START_SBUS_VALUE;
	
	if( (SBUS_ch_value[SBUS_ROBOT_ENABLE_CH] != ROBOT_STOP_SBUS_VALUE) &&
			(SBUS_ch_value[SBUS_ROBOT_ENABLE_CH] != ROBOT_START_SBUS_VALUE)
		)
		return;
	
	ennowvalue = SBUS_ch_value[SBUS_ROBOT_ENABLE_CH];
	
	if(RC_CONTROL != ControlSource)
	{
		enprevalue = ennowvalue;
		return;
	}
	if(ennowvalue != enprevalue)//只锟叫凤拷锟斤拷锟叫憋拷锟绞憋拷锟脚讹拷锟斤拷
	{
		if((ennowvalue == ROBOT_STOP_SBUS_VALUE)&&(Robotstatus == POSE_CONTROL))//stop robot
		{ 
	//		if(Robotstatus != ROBOT_STOP)
	//				Robotstatus = ROBOT_STOP;
				SET_ROBOT_STOP();
		}
		
		if(ennowvalue == ROBOT_START_SBUS_VALUE)//start robot
		{
				SET_ROBOT_START();
	//		if(Robotstatus == ROBOT_STOP)
	//		Robotstatus = ROBOT_INIT;						
		}
	}
	enprevalue = ennowvalue;
}

/*******************************************************************************
* Function Name : DRIVER_CAN_Init
* Description   : Dual motor drivers CAN cmd intialize ,disable->set ->??????->?????????->????123
* Input         : None  
* Output        : NONE
* Return        : NONE
**************************************************************/
void DRIVER_CAN_Init(void)
{
	CANSendMsg(DRIVER_CAN_DisablePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
	CANSendMsg(DRIVER_CAN_SyncModePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
//			rt_thread_delay(100);
	CANSendMsg(DRIVER_CAN_TorqueModePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
	CANSendMsg(DRIVER_CAN_LM_TorqueaimtimePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
//				rt_thread_delay(100);
	CANSendMsg(DRIVER_CAN_RM_TorqueaimtimePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
	CANSendMsg(DRIVER_CAN_Enable06Pack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
//				rt_thread_delay(100);
	CANSendMsg(DRIVER_CAN_Enable07Pack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
	CANSendMsg(DRIVER_CAN_Enable0fPack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);

	rt_kprintf("MotorDriver initalization Success!\n");

}


/*******************************************************************************
* Function Name : DRIVER_CAN_Stop
* Description   : Dual motor driver CAN cmd stop motor
* Input         : None  
* Output        : NONE
* Return        : NONE
**************************************************************/
void DRIVER_CAN_Stop(void)
{
	u8 i;
	memset(&Target_value,0,sizeof(Target_value));
	U16value_Init(SBUS_ch_value,SBUS_VALUE_BASE,16);//sbus??????

	IntegralYawRateDiff(0,0,1);
//	CANSendPacker(0x601,CAN_Id_Standard, DRIVER_CAN_DisablePack, 8);//????
	CANSendMsg(DRIVER_CAN_DisablePack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);
	rt_kprintf("motors stop!\n");

}

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void Robot_setup(void)
{
		MotortStatus = MOTOR_TORQUE_MODE;//MOTOR_SPEED_MODE;//
		Robotstatus = ROBOT_INIT;
		rt_kprintf("Robot status initilize to INIT!\r\n");
		DRIVER_CAN_Init();//
}


void	CloseLoopTorque_Calucate(CONTORL_TARGET * target, App_SensorInfo * realvalue,floatParams* pitch_torquePID,floatParams* torqueDiffPID)
{
//				float targettorque,targetdifftorque;
				//pitch angle -torque loop as inner loop ,put out Target_value.torque
				//calculate target torque
				target->torque = pitch_torquePID->param1 * ( target->pitch_angle  - 	 realvalue->fused_pitch_angle)		+
												 pitch_torquePID->param2 * ( target->pitch_angle_rate -  realvalue->pitch_angle_rate  );
	
//				Target_value.torque = Pitch_TorquePID.param1 * ( Target_value.pitch_angle - 			g_tSysStatus.sensor_info.fused_pitch_angle )+ 
//															Pitch_TorquePID.param2 * ( Target_value.pitch_angle_rate -  g_tSysStatus.sensor_info.pitch_angle_rate  );
				//target torque limit
				if(target->torque > TARGET_TORQUE_LIMIT)
					target->torque = TARGET_TORQUE_LIMIT;
				if(target->torque < -TARGET_TORQUE_LIMIT)
					target->torque = -TARGET_TORQUE_LIMIT;
				
				
				//yaw_rate-Torque?,??Target_value.diff_torque
				//*****yaw??,?????,??????,z???????,??????????,?????????,
				//*****????????,??????,????????,?????,????????????,??????,?pitch?????????
				//*****?????????****
				target->diff_torque = torqueDiffPID->param1 * ( target->yaw_angle_rate - realvalue->gyro_z_raw   ) + //P
															torqueDiffPID->param2 * IntegralYawRateDiff( target->yaw_angle_rate ,realvalue->gyro_z_raw ,0);//I

//				Target_value.diff_torque = YawRate_TorqueDiffPID.param1 * ( Target_value.yaw_angle_rate - g_tSysStatus.sensor_info.gyro_z_raw   ) + //P
//																	 YawRate_TorqueDiffPID.param2 * IntegralYawRateDiff( Target_value.yaw_angle_rate ,( g_tSysStatus.sensor_info.gyro_z_raw  ),0);//I
				//torque merge and put out
				target->MTorque_mA.param1 = 0x0		 + (-1) * (s16)( target->torque + target->diff_torque );
				target->MTorque_mA.param2 =  (s16)( target->torque - target->diff_torque );
//				Target_value.MTorque_mA.param1 = 0x0		 + (-1) * (s16)( targettorque + targetdifftorque );//????????????????????,???
//				Target_value.MTorque_mA.param2 = (s16)( targettorque - targetdifftorque );//??????s16?????u16,????,?????????
				//Build torque Driver CAN sendpackage
				DRIVER_CAN_TorqueSendPack[4] = (u8)( target->MTorque_mA.param1 & 0x00ff );
				DRIVER_CAN_TorqueSendPack[5] = (u8)( target->MTorque_mA.param1 >>8 );
				DRIVER_CAN_TorqueSendPack[6] = (u8)( target->MTorque_mA.param2 & 0x00ff );
				DRIVER_CAN_TorqueSendPack[7] = (u8)( target->MTorque_mA.param2 >>8 );
				//send Torque to Driver
	//			CANSendPacker(0x601,STANDARD_FORMAT, DRIVER_CAN_TorqueSendPack, 8);//send and set dual motor torque
				CANSendMsg(DRIVER_CAN_TorqueSendPack,0x601,CAN_Id_Standard,CAN_RTR_Data,8);//send dual motor speed instruction

}
    //JOINT_STATE joints_state[18] = {0};
		//u16 setvalue =500;
void test_init(void) {
    ////const u8 num = 18;

   // for (u8 i = 0; i < num; i++) {
    ///    u8 id = i ;//+ 1;
        // id, angle, duration, state
        ///set_angle(id, 0, 2.0, &joints_state[i]);
//			setServoPosition(id,setvalue,500);
//			rt_thread_delay(1);
    //}
}


/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
//u32 motioncnt = 0;
	u16 tar_send_pos = 500;
	s16 read_pos_now;
  s16 voltage;
 void MotionTask_entry(void* parameter)
{
	u16 tar_send_pos_new;
	u8 motor_id =0x02;
	
	U16value_Init(SBUS_ch_value,SBUS_VALUE_BASE,16);//sbus rec value initialize 
	//wait 10ms for system setup
	rt_thread_delay(100);//100ms
	
//	test_init();
//	test_leg_move();
	hexapod_test();	
	while(1)
	{
//		motioncnt ++ ;	
//		tar_send_pos_new = 500 + (SBUS_ch_value[1] - SBUS_VALUE_BASE)*5/7;
//		
////		if(abs(tar_send_pos - tar_send_pos_new) >10){
//			tar_send_pos = tar_send_pos_new;
//			setServoPosition(motor_id,tar_send_pos,200);//param: motor id,target pos,duration
////			rt_thread_delay(2);//wait motor move complete
//			read_pos(motor_id, &read_pos_now, 1);//param: motor id ,count
//			rt_kprintf("now motor %d position is %d\n", motor_id,read_pos_now);
//		}
//		test_init();
		readServoVin(motor_id, &voltage, 0x01);
		
		
		rt_sem_take(rt_sem_motion,RT_WAITING_FOREVER);
//		rt_thread_delay(5000);
	}
}



MSH_CMD_EXPORT(SET_ROBOT_START, set robot start to work);
MSH_CMD_EXPORT(SET_ROBOT_STOP, set robot stop);


/**********************************************  END OF FILE motion.c *****************************************************************/
